#!/usr/bin/env python

from gi.repository import GLib, GObject
import socket, struct, time, math
import numpy as np

import pat.frames as fr
import pat.utils  as pu

AHRS_STATE      = 40

sv_ax    = 0
sv_ay    = 1
sv_az    = 2
sv_p     = 3
sv_q     = 4
sv_r     = 5
sv_x     = 6
sv_y     = 7
sv_z     = 8
sv_phi   = 9
sv_theta = 10
sv_psi   = 11
sv_vx    = 12
sv_vy    = 13
sv_vz    = 14
sv_mx    = 15
sv_my    = 16
sv_mz    = 17
sv_size  = 18

class Server(GObject.GObject):

    def __init__(self, ip="127.0.0.1", port=2002):
        GObject.GObject.__init__(self)
        self.s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.s.bind((ip, port))
        GObject.io_add_watch(self.s, GObject.IO_IN, self.on_message_received)
        self.client = None
    
    def on_message_received(self, source, cb_condition):
        data, addr = source.recvfrom(1024)
        print "on_message_received from", addr
        self.client = addr
        return True

    def send(self, X):
        if self.client <> None:
            _type = AHRS_STATE
            tv_usec, tv_sec = math.modf(time.time())
            tv_sec = int(tv_sec); tv_usec = int(tv_usec*1e6)
            buf = struct.pack(">LLLLL", tv_sec, 0, tv_usec, 0, _type)
            data = struct.pack("ddd ddd ddd ddd ddd ddd", 0.,0.,0., 0.,0.,0.,
                               pu.ft_of_m(X[fr.SixDOFfEuclidianEuler.x]),
                               pu.ft_of_m(X[fr.SixDOFfEuclidianEuler.y]),
                               pu.ft_of_m(X[fr.SixDOFfEuclidianEuler.z]),
                               X[fr.SixDOFfEuclidianEuler.phi],
                               X[fr.SixDOFfEuclidianEuler.theta],
                               X[fr.SixDOFfEuclidianEuler.psi],
                               0.,0.,0., 0.,0.,0.)
            buf=''.join([buf, data])
            self.s.sendto(buf, self.client)
            #print "send"
    




if __name__ == "__main__":

    def periodic_callback(arg):
        X = np.zeros(fr.SixDOFfEuclidianEuler.size)
        X[fr.SixDOFfEuclidianEuler.phi] = np.sin(time.time())
        X[fr.SixDOFfEuclidianEuler.x] = 1.
        srv.send(X)
        return True
    
    srv = Server()
    GObject.timeout_add(int(1000./25.), periodic_callback, None)
    mainloop = GLib.MainLoop()
    mainloop.run()

